#pragma once

#ifndef _FRAME_H_
#define _FRAME_H_

#include "common_include.h"
#include"utility.h"
#include"state.h"

namespace fast_lio{

struct Frame{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Frame> Ptr;

    unsigned long keyframe_id_ =0;
    unsigned long frame_id_ =0;
    double time_stamp_;
    bool is_keyframe_ = false;
    bool is_get_imu_pose_estimate_ = false;

    deque<sensor_msgs::Imu::ConstPtr> imu_deq_;

    State::Ptr p_state_;

    pcl::PointCloud<PointType>::Ptr pc_surf_,pc_full_;

    mutex w_pose_mutex_,imu_pose_mutex_;

    Frame(){}
    Frame(double time_stamp,pcl::PointCloud<PointType>::Ptr pc){
        time_stamp_ = time_stamp;
        pc_surf_ = pc;
        pc_full_.reset(new pcl::PointCloud<PointType>);
        pcl::copyPointCloud(*pc,*pc_full_);
        pcDownSample(pc_surf_, 0.5);

        p_state_.reset(new State);

        static long frame_factory_id = 0;
        frame_id_ = frame_factory_id++;
    }

    void setKeyFrame(){
        static long keyframe_factory_id = 0;
        is_keyframe_ = true;
        keyframe_id_ = keyframe_factory_id++;
    }

    SE3 getPose_world_imu(){
        // unique_lock<mutex> lck(w_pose_mutex_);
        return SE3(p_state_->q_,p_state_->pos_);
    }

    State getState(){
        // unique_lock<mutex> lck(w_pose_mutex_);
        return *p_state_;
    }

    void setState(const State &state) {
        // unique_lock<mutex> lck(w_pose_mutex_);
        *p_state_ = state;
    }

    PointType getFramePosition(){
        PointType pt;
        SE3 pose = getPose_world_imu();
        pt.x = pose.translation()[0];
        pt.y = pose.translation()[1];
        pt.z = pose.translation()[2];
        return pt;
    }

};



}
#endif  
